/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <opencv/highgui.h>

#include "define.h"
#include "cchessmaster.h"
#include "chesserror.h"
#include "katanavision/katanafuncs.h"

using namespace Katana;
using namespace Chess;
using namespace std;

CChessMaster::CChessMaster(
	Katana::CKatanaWithEye& _arm, 
	CChessDetector& _det, 
	CFigureDetector& _fig, 
	CGnuchessEnvelop& _engine)
:
	arm(_arm), 
	det(_det), 
	fig(_fig), 
	src(_arm.GetEye()),
	katana(_arm.GetKatana()),
	chess_engine(_engine)
{
	drop_point.x = 300; drop_point.y = 200; drop_point.z = -100;
	tablez = -186.5;
	fheight = 80.0;
	prev_rotation = -1;
	pos.InitialPosition();
}

void CChessMaster::Start() {
	chess_engine.Start();
	pos.InitialPosition();
}

int CChessMaster::PossibleMoves(const CPosition& recogn_pos, int max_diff, MoveList& moves) {
	FigureList appear, disappear, change;
	int diff = recogn_pos.Difference(pos, disappear, appear, change);
	SMove mv;
	// слишком большие различия
	if (diff > max_diff) return diff;
	
	for(FigureList::iterator d = disappear.begin(); d != disappear.end(); d++) 
		for(FigureList::iterator a	 = appear.begin(); a != appear.end(); a++) {
			mv.from = *d; 
			mv.to = *a;
			if (chess_engine.CheckMove(mv)) {
				CPosition tmp = pos; tmp.Move(mv);
				moves.push_back(SMoveDiff(mv, recogn_pos.Difference(tmp)));
			}
		}
	for(FigureList::iterator d = disappear.begin(); d != disappear.end(); d++) 
		for(FigureList::iterator c	 = change.begin(); c != change.end(); c++) {
			mv.from = *d; 
			mv.to = *c;
			if (chess_engine.CheckMove(mv)) {
				CPosition tmp = pos; tmp.Move(mv);
				moves.push_back(SMoveDiff(mv, recogn_pos.Difference(tmp)));
			}
		}
	moves.sort();
	return diff;
}

bool CChessMaster::RecogniseMove(SMove& move) {
	img = cv::Mat(src.GrabFrame(), true);
	IplImage i = img; 
	
	cout << "Detect chessboard ... ";
	if (!det.DetectChessboard(&i, cells)) throw Errors::CBoardDetectionError();
	cout << "done." << endl << "Detect figures ... ";
	recogn_pos.Clear();
	if (!fig.DetectFigures(img, cells, recogn_pos)) throw Errors::CFigureDetectionError();
	cout << "done." << endl;
	
	cv::Mat img_move = img.clone();
	recogn_pos.DrawPosition(img_move, cells);
	cvNamedWindow("Move", 1);
	cv::imshow("Move", img_move);
	cv::waitKey(100);
	
	float angle = 0.0f;
	int diff, best_diff = 1000, good_moves_count = 0;
	int rotation_cand, best_rot;
	bool move_detected;
	MoveList moves;
	MoveList::iterator second;
	
	for(int i = 0; i < 4; i++) {
		moves.clear();
		diff = PossibleMoves(recogn_pos, 6, moves);
		
		cout << "Rotation " << i << " Diff: " << diff;
		cout << " Possible moves: ";
		for(MoveList::iterator k = moves.begin(); k != moves.end(); k++) 
			cout << " " << string(k->move) << "(" << k->diff << ")";
		cout << endl;
		
		if (diff < best_diff) {
			best_diff = diff; 
			best_rot = i;
		}
		
		if (moves.size() > 0)
			if (moves.size() == 1 || (++(second = moves.begin()))->diff > moves.begin()->diff) {
				move = moves.begin()->move; 
				good_moves_count++; 
				rotation_cand = i;
			}

		recogn_pos.RotateHalfPI();
	}
	
	if (best_diff == 0 && (prev_rotation == best_rot || good_moves_count != 1)) {
		prev_rotation = best_rot;
		cout << "THERE IS NO MOVE" << endl;
		move_detected = false;
	}
	else if (good_moves_count == 1) {
		prev_rotation = rotation_cand;
		cout << "HUMAN MOVE: " << string(move) << endl;
		move_detected = true;
	}
	else { 
		cells.Rotate(best_rot*PI/2);
		for(int i = 0; i < best_rot; i++) recogn_pos.RotateHalfPI();
		throw Errors::CHumanMoveError(best_diff);
	}
	
	cells.Rotate(prev_rotation*PI/2);
	for(int i = 0; i < prev_rotation; i++) recogn_pos.RotateHalfPI();
	img_move = img.clone();
	recogn_pos.DrawPosition(img_move, cells);
	cv::imshow("Move", img_move);
	cv::waitKey(100);
	
	return move_detected;
}

bool CChessMaster::HumanMove() {
	SMove move;
	bool human_made_move;
	
	katana.moveRobotTo(0, 400, 0, 0,0,0, true);
	
	human_made_move = RecogniseMove(move);
	if (human_made_move) {
		pos.Move(move);
		chess_engine.Move(move);
	}
	fig.TrainSample(img, cells, pos);	
	return human_made_move;
}

void CChessMaster::MoveCoordinatesCorrection(const cv::Point3f& from, cv::Point3f& to) {
	KNI::KatanaKinematics::encoders enc(5);
	KNI::KatanaKinematics::coordinates pose(6);
	double phi1, delta, r;
	
	pose[0] = from.x; pose[1] = from.y; pose[2] = from.z + fheight;
	pose[3] = 0; pose[4] = 0; pose[5] = 0;
	katana.getKinematics().IK(enc.begin(), pose, enc);
	katana.getKinematics().DK(pose, enc);
	phi1 = pose[3];
	
	pose[0] = to.x; pose[1] = to.y; pose[2] = to.z + fheight;
	pose[3] = 0; pose[4] = 0; pose[5] = 0;
	katana.getKinematics().IK(enc.begin(), pose, enc);
	katana.getKinematics().DK(pose, enc);

	delta = pose[3] - phi1;
	r = sqrt(to.x*to.x + to.y*to.y);
	delta = (r - fheight*delta) / r;
	to.x *= delta; to.y *= delta;
}

void CChessMaster::MoveFigure(cv::Point3f from, cv::Point3f to) {
	
	cout << "Move figure from " << from << " to " << to;
	MoveCoordinatesCorrection(from, to);
	cout << " { " << to << " } " << endl;
	
	katana.moveRobotTo(from.x, from.y, tablez + 2*fheight + 30, 0,0,0, true);
	katana.moveRobotTo(from.x, from.y, tablez + fheight + 15, 0,0,0, true);
	GrabDotObject(katana, 15, 15, 5000); 
	
	PseudoLinearMove(katana, Point3d(from.x, from.y, tablez + 2*fheight + 30));
	katana.moveRobotTo(to.x, to.y, tablez + 2*fheight + 30, 0,0,0, true);
	
	PseudoLinearMove(katana, Point3d(to.x, to.y, tablez + fheight));
	katana.movDegrees(M_GRP, 15, true);
	katana.moveRobotTo(to.x, to.y, tablez + 2*fheight + 30, 0,0,0, true);
}
	
void CChessMaster::ComputerMove() {
	SMove move, rook_move;
	cv::Point3f from, to;

	cout << "Thinking ..." << endl;
	chess_engine.Go(move);
	cout << "MY MOVE: " << string(move) << endl;
	
	to =  arm.GetTransform().ReverseProjection(cells.Cell(move.to), HPlane3f(0,0,-1,tablez)); 
	if (pos.Cell(move.to) != empty) { // the move is a capture
		from = to;
		to = drop_point;
		drop_point.x -= 20;
		MoveFigure(from, to);
		to = from;
	}
	from = arm.GetTransform().ReverseProjection(cells.Cell(move.from), HPlane3f(0,0,-1,tablez));
	MoveFigure(from, to);
	
	if (pos.CheckCastle(move, rook_move)) { //this move is a castle
		from = arm.GetTransform().ReverseProjection(cells.Cell(rook_move.from), HPlane3f(0,0,-1,tablez));
		to =  arm.GetTransform().ReverseProjection(cells.Cell(rook_move.to), HPlane3f(0,0,-1,tablez)); 
		MoveFigure(from, to);
	}
	
	pos.Move(move);
	
	katana.moveRobotTo(0, 400, 0, 0,0,0, true);
}

